#include "AvoidObstacle.h"
#include "../Vectores.h"

#include <vector>

using namespace std;

AvoidObstacle::AvoidObstacle(){
}

Steering* AvoidObstacle::getSteering(){
    Steering *ret = new Steering();
  /*
  int i, w, h;
  float vel[3], collision[3], normal[3];
  bool choco = false;
  normalize2(character->velocity, vel);
//    cout << "vel " << vel[0] << "," << vel[1] << endl;
  // Lanzar el rayo
  for(i=0; i<lookahead; i += 4){
    if (character->position == NULL)
      exit(0);
    // El grosor de cada cuadro del mapa es de 40 pixeles
    w = ((vel[0] * i) / 40 );//+ character->position[0];
    h = ((vel[1] * i) / 40 );//+ character->position[1];
    if( (*this->mapa)[w][h] == 1){
      collision[0] = vel[0] * i;
      collision[1] = vel[1] * i;
      collision[2] = 0;
      /// Va a chocar! Calculamos el vector normal a la superficie
      /// TODO: calcular una buena normal
      // int e = 4;
      normal[0] = -vel[0];
      normal[1] = -vel[1];
      normal[2] = 0;
      normalize(normal);
      /// Calculamos el target imaginario
      multVectorScalar2(normal, distance, normal);
      sumarVectores(collision, normal, target->position);
      cout << "choco" << endl;
      choco = true;
      break;
    }
//      cout << " mapa[" << w << "]["<< h <<"] " << (*this->mapa)[w][h] << endl;
  }
  if(choco)
    return Seek::getSteering();
  else{
    Steering* ret = new Steering();
//      cout << " steering->vel" << ret->linear[0] << "," << ret->linear[1] << endl;
    */
    return ret;
  }